!/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!*  Copyleft (!c) 2004-2005 Div. Nucl. Med., Cyric, Tohoku Univ.    *
!*  Copyleft (!c) 2005-2011 Elseif Laboratory.                      *
!*  elseifkk@users.sf.net                                           *
!*                                                                  *
!*  All Rights Reversed.                                            *
!*                                                                  *
!*                                                                  *
!*  LICENSE AGREEMENT:                                              *
!*                                                                  *
!*  You agree that we don't agree to anything.                      *
!*                                                                  *
!*                                                                  *
!*  DISCLAIMER OF WARRANTIES:                                       *
!*                                                                  *
!*  This software is provided to you "AS IS," and the authors and   *
!*  contributors disclaim any warranty or liability obligations to  *
!*  you of any kind, implied or express, including but not limited  *
!*  to the warranties of merchantability, fitness for a particular  *
!*  purpose, and non-infringement.                                  *
!* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
!
! general optimization utility
!
! TTD: 
!   1) there must be much better method than things like this.
! note: 
!   none.
!
module optimizer
  implicit none
  integer*4,private,parameter::NUM_ITERATION_MAX=100000
contains
  !
  subroutine golden(f,a0,b0,n,x0,d,eps,x_out,fmin_out)
    real*8,external::f
    real*8,intent(in)::a0,b0
    integer*4,intent(in)::n
    real*8,intent(in)::x0(n)
    real*8,intent(in)::d(n)
    real*8,intent(in)::eps
    real*8,intent(out)::x_out(n)
    real*8,intent(out)::fmin_out
    real*8,parameter::tau= .6180339887498948482045868343656381177203091798057
    real*8,parameter::ftau=.3819660112501051517954131656343618822796908201942
    real*8 f_lmd,f_mu
    real*8 lmd,mu
    real*8 a,b

    a=a0
    b=b0

    lmd=a+ftau*(b-a)
    mu=a+tau*(b-a)

    f_lmd=f(x(lmd))
    f_mu=f(x(mu))

    do

       if(abs(a-b)<eps) then
          if(f_lmd<f_mu) then
             x_out=x(lmd)
             fmin_out=f_lmd
          else
             x_out=x(mu)
             fmin_out=f_mu
          end if
          exit
       end if

       if( f_lmd < f_mu ) then
          b=mu
          mu=lmd
          lmd=a+ftau*(b-a)
          f_mu=f_lmd
          f_lmd=f(x(lmd))
       else
          a=lmd
          lmd=mu
          mu=a+tau*(b-a)
          f_lmd=f_mu
          f_mu=f(x(mu))
       endif

    end do

  contains

    function x(alpha)
      real*8,intent(in)::alpha
      real*8 x(n)
      x=x0+alpha*d
    end function x

  end subroutine golden
  
  !-----------------------------------------------------------------------------!
  subroutine simplex(obj,n,x0,xmin,xmax,&
       errabs_min,errrel_min,errrelobj_min, &
       errabs,errrel,errrelobj,xout,rc_out,minobj_out,num_ite)
    integer*4,intent(in)::n            ! number of parameter
    real*8,intent(in)::x0(n)           ! initial guess
    real*8,intent(in)::xmin(n),xmax(n) ! boundaries
    real*8,intent(in)::errabs_min      ! lower limit of params abs. diff**2
    real*8,intent(in)::errrel_min      ! lower limit of params rel. diff**2
    real*8,intent(in)::errrelobj_min   ! lower limit of objs rel. diff
    real*8,intent(out)::errabs         ! params abs. diff at the last iter.
    real*8,intent(out)::errrel         ! params rel. diff at the last iter.
    real*8,intent(out)::errrelobj      ! objs rel. diff at the last iter.
    real*8,intent(out)::xout(n)        ! solution
    real*8,external::obj
    integer*4,intent(out),optional::rc_out
    real*8,intent(out),optional::minobj_out
    integer*4,intent(out),optional::num_ite
    real*8,allocatable,target::s(:,:)
    real*8,allocatable::cms(:)
    real*8,allocatable::val(:)
    real*8,pointer::stop(:),sbot(:)
    real*8 d
    integer*4 ns,k,is,ip
    real*8 val_r,val_e
    integer*4 istop,isbot,ismed
    real*8 max,min,sum_val
    real*8,allocatable:: sr(:),se(:)
    real*8,allocatable::errrels(:)
    integer*4 rc

real*8 g,g_max
integer*4 ig_max

    istop=0
    isbot=0
    ns=2*n+1
    errabs=0
    rc=-1
    allocate(s(n,ns),cms(n),val(ns),sr(n),se(n),errrels(n))
    nullify(stop,sbot)
    do is=1,ns
       s(:,is)=x0
    enddo
    do ip=1,n
       is=2*ip
       s(ip,is)=s(ip,is)+xmin(ip)
       s(ip,is-1)=s(ip,is-1)+xmax(ip)
    enddo
    do is=1,ns
       val(is)=obj(s(:,is))
    end do
    !
    do k=1,NUM_ITERATION_MAX
       ! sorting costs
       max=-HUGE(0.d0)
       min=HUGE(0.d0)
       do is=1,ns
          if(max<val(is))then
             istop=is
             max=val(is)
             stop=>s(:,is)
          end if
          if(min>val(is)) then
             isbot=is
             min=val(is)
             sbot=>s(:,is)
          endif
       end do
       ! convergence ?
       if(errabs_min/=0.and.get_errabs()<errabs_min) then
          rc=1
          exit
       end if
       !
       if(errrel_min/=0.and.get_errrel()<errrel_min) then
          rc=2
          exit
       end if
       ! 
       if(errrelobj_min/=0.and.get_errrelobj()<errrelobj_min) then
          rc=3
          exit
       endif
       !
       if(min==max) then
          rc=4
          exit
       endif
       ! set cm
       cms=0
       sum_val=0
       do is=1,ns
          if(is==istop) cycle
          do ip=1,n
             cms(ip)=cms(ip)+s(ip,is)
          end do
          sum_val=sum_val+1
       end do
       cms=cms/sum_val
       !
       sr=-stop+2*cms
       val_r=obj(sr)
       if(val_r<val(isbot))then
          d=2
          do
             d=d+1
             se=(1-d)*stop+d*cms
             val_e=obj(se)
             if(val_e<val_r) then
                val_r=val_e
                sr=se
                cycle
             endif
             stop=sr
             val(istop)=val_r
             exit
          enddo
          cycle
       else
          !
          max=-HUGE(0.d0)
          ismed=0
          do is=1,ns
             if(is==istop) cycle
             if(max<val(is)) then
                ismed=is
                max=val(is)
             end if
          end do
          !
          if(val_r<val(ismed))then
             stop=sr
             val(istop)=val_r
             cycle
          endif
          !
          if(val_r<val(istop)) then
             stop=sr
             val(istop)=val_r
          endif
          sr=(-stop+3*cms)/2
          val_r=obj(sr)
          if(val_r<val(ismed))then
             stop=sr
             val(istop)=val_r
             cycle
          endif
       endif
       !
       sr=(stop+cms)/2
       val_r=obj(sr)
       if(val_r<val(ismed)) then
          stop=sr
          val(istop)=val_r
          cycle
       endif
       do is=1,ns
          if(is==isbot) cycle
          s(:,is)=(sbot+s(:,is))/2
          val(is)=obj(s(:,is)) 
       end do
    end do

    xout=sbot
    errabs=get_errabs()
    errrel=get_errrel()

g_max=0.d0
ig_max=0

       do is=1,ns
          if(is==isbot) cycle
          d=sqrt(sum((s(:,is)-s(:,isbot))**2))
          if(d/=0) then
             g=(val(is)-val(isbot))/d
             if(g>g_max) then
                g_max=g
                ig_max=is
             end if
          end if
       end do

if(ig_max/=0) then
call golden(obj,-1.d0,1.d0,n,sbot,s(:,ig_max)-sbot,1.d-4,xout,min)
endif

    errrelobj=get_errrelobj()
    if(present(rc_out)) rc_out=rc
    if(present(minobj_out)) minobj_out=min
    if(present(num_ite)) num_ite=k
    nullify(stop,sbot)
    deallocate(s,cms,val,sr,se,errrels)
  contains
    real*8 function get_errabs()
      get_errabs=maxval(abs(stop-sbot))
    end function get_errabs
    !
    real*8 function get_errrel()
      where(stop/=0)
         errrels=abs(sbot/stop-1)
      elsewhere
         errrels=-1
      end where
      get_errrel=maxval(errrels)
      if(get_errrel<0) get_errrel=huge(1.d0)        
    end function get_errrel
    !
    real*8 function get_errrelobj()
      if(max/=0)then
         get_errrelobj=abs(min/max-1)
      else
         get_errrelobj=huge(1.0d0)
      end if
    end function get_errrelobj
end subroutine simplex
!-----------------------------------------------------------------------------!
  subroutine gnewton(obj,n,x0,dx,lambda,&
       errabs_min,errrel_min,errrelobj_min,&
       errabs,errrel,errrelobj,xout,&
       rc_out,minobj_out,num_ite)
    integer*4,intent(in)::n
    real*8,intent(in)::x0(n)
    real*8,intent(in)::dx(n)
    real*8,intent(in)::lambda
    real*8,intent(in)::errabs_min
    real*8,intent(in)::errrel_min
    real*8,intent(in)::errrelobj_min
    real*8,intent(out)::errabs
    real*8,intent(out)::errrel
    real*8,intent(out)::errrelobj
    real*8,intent(out)::xout(n)
    integer*4,intent(out),optional::rc_out
    real*8,intent(out),optional::minobj_out
    integer*4,intent(out),optional::num_ite
    real*8,external::obj
    real*8,allocatable::M(:,:),w(:),v(:),Mw(:)
    real*8,allocatable::a(:,:),dab(:),mu(:),lmd(:),dfM(:)
    real*8,allocatable::f(:),f1(:),df(:),x1(:,:),x_1(:),df_1(:),dx_inv(:),x(:)
    real*8,allocatable::lm(:,:)! lmd, mu
    real*8 flm(2) ! flmd, fmu
    real*8 t(2)
    integer*4 ilm,iml,rc
    real*8 wMw,vv,f_1,f_2,s
    real*8,parameter::tau= .6180339887498948482045868343656381177203091798057
    real*8,parameter::ftau=.3819660112501051517954131656343618822796908201942
    real*8 f0
    real*8,allocatable::errrels(:)
    integer*4 ip,jp,k,kk
    real*8 norm
    t(2)=tau
    t(1)=1-tau
    allocate(x(n),x_1(n),M(n,n),x1(n,n),df(n),df_1(n),f(n),f1(n),v(n),w(n),&
         Mw(n),a(n,2),dab(n),lmd(n),mu(n),dfM(n),dx_inv(n),lm(n,2),errrels(n))
    x=x0
    M=0
    dx_inv=1/dx
    do ip=1,n
       M(ip,ip)=1
    end do
    f0=obj(x)

    call set_df
    !
    do k=1,NUM_ITERATION_MAX
       a(:,1)=x ! initial point
       dfM=0
       do ip=1,n
          do jp=1,n
             dfM(ip)=dfM(ip)+df(jp)*M(jp,ip)
          end do
       end do

       norm=sqrt(sum(dfM**2))
       if(norm==0) then
          rc=5
          exit
       end if
       norm=lambda/norm
       do ip=1,n
          a(ip,2)=a(ip,1)-dfM(ip)*norm ! end point
       end do
       dab=a(:,2)-a(:,1)

       lm(:,1)=a(:,1)+t(1)*dab ! update lmd, mu
       lm(:,2)=a(:,1)+t(2)*dab ! update lmd, mu

       flm(1)=obj(lm(:,1))
       flm(2)=obj(lm(:,2))

       ! update x
       x_1=x
       f_2=f0
       call line_search_gs

       if(f0>f_2) then
          ! line search failed; already at the bottom
          x=x_1
          f0=f_2
          rc=-2
          exit
       endif
       dab=x-x_1
       f_1=f_2
       if(isConverged()) exit
       ! get df
       df_1=df
       call set_df
       ! update M
       if(.not.update_M()) exit
    end do

    xout=x
    errabs=get_errabs()
    errrel=get_errrel()
    errrelobj=get_errrelobj()
    if(present(rc_out)) rc_out=rc
    if(present(minobj_out)) minobj_out=f0
    if(present(num_ite)) num_ite=k    
    deallocate(x,x_1,M,x1,df,df_1,f,f1,v,w,Mw,a,dab,lmd,mu,dfM,dx_inv,errrels)
  contains
    subroutine set_df
      do ip=1,n
         x1(:,ip)=x
         x1(ip,ip)=x1(ip,ip)+dx(ip)
         f1(ip)=obj(x1(:,ip))
         df(ip)=(f1(ip)-f0)*dx_inv(ip)
      end do
    end subroutine set_df
    !
    subroutine line_search_gs
      do kk=1,NUM_ITERATION_MAX
         f_1=f0
         if(flm(1)<flm(2)) then
            ilm=1
            iml=2
         else
            ilm=2
            iml=1
         endif
         x=lm(:,ilm)
         f0=flm(ilm)
         a(:,iml)=lm(:,iml)  ! b=mu, a=lmd
         lm(:,iml)=lm(:,ilm) ! mu=lmd, lmd=mu
         flm(iml)=flm(ilm)   ! fmu=flmd, flmd=fmu
         dab=a(:,2)-a(:,1) 
         lm(:,ilm)=a(:,1)+t(ilm)*dab ! update lmd, mu
         if(kk>3) then
            if(isConverged()) return
         end if
         flm(ilm)=obj(lm(:,ilm)) ! update flmd, fmu
      end do
    end subroutine line_search_gs
    !   
    real*8 function get_errabs()
      get_errabs=maxval(abs(dab))
    end function get_errabs
    !
    real*8 function get_errrel()
      where(x/=0)
         errrels=abs(dab/x)
      elsewhere
         errrels=-1
      end where
      get_errrel=maxval(errrels)
      if(get_errrel<0) get_errrel=huge(1.d0)        
    end function get_errrel
    !
    real*8 function get_errrelobj()
      if(f0/=0) then
         get_errrelobj=abs((f_1-f0)/f0)
      else
         get_errrelobj=huge(1.d0)
      end if
    end function get_errrelobj
    !
    logical*4 function isConverged()
      isConverged=.TRUE.
      rc=1
      if(f0==f_1) return 
      rc=2
      if(errabs_min/=0.and.get_errabs()<errabs_min) return
      rc=3
      if(errrel_min/=0.and.get_errrel()<errrel_min) return
      rc=4
      if(errrelobj_min/=0.and.get_errrelobj()<errrelobj_min) return
      rc=-1
      isConverged=.FALSE.
    end function isConverged
    !
    logical*4 function update_M()
      update_M=.FALSE.
      v=x-x_1
      w=df-df_1
      vv=0
      vv=sum(v**2)
      rc=6
      if(vv==0) return
      wMw=0
      Mw=0
      s=0
      do ip=1,n
         do jp=1,n
            s=M(jp,ip)*w(jp)
            wMw=wMw+w(ip)*s
            Mw(ip)=Mw(ip)+s
         end do
      end do
      rc=7
      if(wMw==0) return
      do ip=1,n
         do jp=1,ip
            M(jp,ip)=M(jp,ip)+v(ip)*v(jp)/vv-Mw(jp)*Mw(ip)/wMw
         end do
      end do
      do ip=1,n
         do jp=ip+1,n
            M(jp,ip)=M(ip,jp)
         end do
      end do
      update_M=.TRUE.
    end function update_M
    !
  end subroutine gnewton
  !
end module optimizer
